#include "stm32f10x.h"                  // Device header
#include "delay.h"
#include "oled.h"
#include "mpu6050.h"
#include "serial.h"
#include "string.h"
#include "stdlib.h"
#include "led.h"
#include "ad.h"
#include "tb6612.h"


uint8_t ID;
int16_t AX, AY, AZ, GX, GY, GZ;

char String[100];

void mpu6050_test(void)
{
	MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
	OLED_ShowSignedNum(2, 1, AX, 5);
	OLED_ShowSignedNum(3, 1, AY, 5);
	OLED_ShowSignedNum(4, 1, AZ, 5);
	OLED_ShowSignedNum(2, 8, GX, 5);
	OLED_ShowSignedNum(3, 8, GY, 5);
	OLED_ShowSignedNum(4, 8, GZ, 5);	
}

void serial_test(void)
{
	sprintf(String, "ID %d", ID);
	Serial_SendString(String);
	
	sprintf(String, "\r\n%d, %d,  %d, %d, %d, %d", AX, AY, AZ, GX, GY, GZ);
	Serial_SendString(String);	
}

void serial_rx_tx_test(void)
{
	if (Serial_RxFlag == 1)
	{
		OLED_ShowString(4, 1, "                ");
		OLED_ShowString(4, 1, Serial_RxPacket);
		
		if (strcmp(Serial_RxPacket, "LED_ON") == 0)
		{
			LED1_ON();
			Serial_SendString("LED_ON_OK\r\n");
			OLED_ShowString(2, 1, "                ");
			OLED_ShowString(2, 1, "LED_ON_OK");
		}
		else if (strcmp(Serial_RxPacket, "LED_OFF") == 0)
		{
			LED1_OFF();
			Serial_SendString("LED_OFF_OK\r\n");
			OLED_ShowString(2, 1, "                ");
			OLED_ShowString(2, 1, "LED_OFF_OK");
		}
		else
		{
			Serial_SendString("ERROR_COMMAND\r\n");
			OLED_ShowString(2, 1, "                ");
			OLED_ShowString(2, 1, "ERROR_COMMAND");
		}
		
		Serial_RxFlag = 0;
	}
}

void serial_motor_cmd_prase(void)
{
	char cmd[3] = {0};
	char head = 0;
	uint8_t speed = 0;
	
	if (Serial_RxFlag == 1)
	{
		OLED_ShowString(4, 1, "                ");
		OLED_ShowString(4, 1, Serial_RxPacket);
		
		head   = Serial_RxPacket[0];
		cmd[0] = Serial_RxPacket[1];
		cmd[1] = Serial_RxPacket[2];
		cmd[2] = Serial_RxPacket[3];
		speed = atoi(cmd);
		
		OLED_ShowSignedNum(4, 8, speed, 3);
		
		if (strcmp(&head, "L") == 0)
		{
			Serial_SendString("Left: \r\n");
			
			Motor1_Go();
			Motor2_Go();
			PWM1_SetDuty(speed);
			PWM2_SetDuty(speed);
		}
		else if (strcmp(&head, "R") == 0)
		{
			Serial_SendString("Right: \r\n");
			
			Motor1_Go();
			Motor2_Go();
			PWM1_SetDuty(speed);
			PWM2_SetDuty(speed);
		}
		else if (strcmp(&head, "G") == 0)
		{
			Serial_SendString("Go: \r\n");
			
			Motor1_Go();
			Motor2_Go();
			PWM1_SetDuty(speed);
			PWM2_SetDuty(speed);
		}
		else if (strcmp(&head, "B") == 0)
		{
			Serial_SendString("Back: \r\n");
			
			Motor1_Reverse();
			Motor2_Reverse();
			PWM1_SetDuty(speed);
			PWM2_SetDuty(speed);
		}
		else if (strcmp(&head, "S") == 0)
		{
			Serial_SendString("Stop: \r\n");
			
			Motor1_Stop();
			Motor2_Stop();
			PWM1_SetDuty(0);
			PWM2_SetDuty(0);
		}
		
		Serial_RxFlag = 0;
	}
}

uint16_t ret_val = 0;

uint8_t pwm_duty = 0;


int main(void)
{
	Serial_Init();
	LED_Init();
    OLED_Init();
//    MPU6050_Init();
//	AD_Init();
//	TB6612_Init();
	
//	OLED_ShowString(1, 1, "ID:");
//    ID = MPU6050_GetID();
//    OLED_ShowHexNum(1, 4, ID, 2);
//	
//	sprintf(String, "ID %d", ID);
//	Serial_SendString(String);
	
	OLED_ShowString(1, 1, "TxPacket");
	OLED_ShowString(3, 1, "RxPacket");

    while (1)
    {
//		ret_val = AD_GetValue();
//		
//		PWM2_SetDuty(pwm_duty);
//		PWM1_SetDuty(pwm_duty);
//		
//		Delay_ms(1000);
//		
//		if(pwm_duty < 100)
//		{
//			pwm_duty += 10;
//		}
//		else
//			pwm_duty = 0;

//		mpu6050_test();
		
		//serial_test();
		
//		serial_rx_tx_test();
		
		serial_motor_cmd_prase();
    }
}
